#include <iostream>
#include <vector>

#include "coroutine.h"
#include <stdio.h>
#include <iostream>
#include <thread>
using namespace std;
using namespace std::this_thread;
using namespace std::chrono;

// #include "DealImage.h"
#include "flow_plugins.h"

#include <msg.pb.h>
typedef pack_camera_move::PosMove Msg; 


void print_now() 
{
    auto tp = system_clock::now();
    auto tt = std::chrono::system_clock::to_time_t(tp);
    auto tm = std::tm{0};
    gmtime_r(&tt, &tm);
    std::chrono::duration<double> sec = tp -
            std::chrono::system_clock::from_time_t(tt) +
            std::chrono::seconds(tm.tm_sec);
    cout  << "now:" << sec.count() << endl;
}


#include "opencv2/core/core.hpp"
#include "opencv2/imgproc/imgproc.hpp"
#include "opencv2/highgui/highgui.hpp"
#include "opencv2/video/video.hpp"
#include "opencv2/gpu/gpu.hpp"

#include "include///timer.h"

#include "serialport_send_lib/serialportwriter.h"

using namespace std;
using namespace cv;
using namespace cv::gpu;

template <typename T>
vector<size_t> sort_indexes(const vector<T> &v) {

    // initialize original index locations
    vector<size_t> idx(v.size());
    iota(idx.begin(), idx.end(), 0);

    // sort indexes based on comparing values in v
    sort(idx.begin(), idx.end(),
         [&v](size_t i1, size_t i2) {return v[i1] < v[i2];});

    return idx;
}

template <typename T>
vector<size_t> GetXY(vector<T> &v, T range)
{
    vector<size_t> indexArr = sort_indexes<T>(v);
    sort(v.begin(), v.end());

    int head = 0, tail = 0;
    int max_peak_count = 0;
    int peak_index = 0;
    while(tail < v.size()){
        if (tail + 1 - head > max_peak_count) {
            max_peak_count = tail + 1 - head;
            peak_index = head;
        }
        ++tail;
        if(tail < v.size()){
            while (v[tail] - v[head] > range) {
                ++head;
            }
        }
    }
    int iBeginIdx = peak_index;
    while (iBeginIdx < v.size() && fabsf(v[iBeginIdx] - v[peak_index]) <= range ){
        iBeginIdx++;
    }
    vector<size_t> tmp(indexArr.begin() + peak_index, indexArr.begin() + iBeginIdx);
    return tmp;
}

static void download(const GpuMat& d_mat, vector<Point2f>& vec)
{
    vec.resize(d_mat.cols);
    Mat mat(1, d_mat.cols, CV_32FC2, (void*)&vec[0]);
    d_mat.download(mat);
}

static void download(const GpuMat& d_mat, vector<uchar>& vec)
{
    vec.resize(d_mat.cols);
    Mat mat(1, d_mat.cols, CV_8UC1, (void*)&vec[0]);
    d_mat.download(mat);
}

static Point getXY(const vector<Point2f>& prevPtsOri, const vector<Point2f>& nextPtsOri, const vector<uchar>& status,  int iLenDisMin, float& fRatio)
{
    float fSumX = 0.f;
    float fSumY = 0.f;
    int iNum = 0;

    vector<float> arrFlow;
    vector<Point2f> prevPts;
    vector<Point2f> nextPts;

    for (size_t i = 0; i < prevPtsOri.size(); ++i){
        if (status[i]){

            Point p = prevPtsOri[i];
            Point q = nextPtsOri[i];

            prevPts.push_back(p);
            nextPts.push_back(q);

            arrFlow.push_back(sqrt((p.x - q.x)*(p.x - q.x)+(p.y - q.y)*(p.y - q.y)));
        }
    }

    vector<float> arrFlowtmp = arrFlow;
    auto arrIndex = GetXY<float>(arrFlow, iLenDisMin);

    //test
    // {
    //     //cout << "get dense part:" << endl;
    //     for(auto& i : arrIndex) {
    //         //cout << (&i - &arrIndex[0]) << ":" << arrFlowtmp[i] << "    ";
    //     }

    //     //cout << endl;

    //     sort(arrFlow.begin(), arrFlow.end());

    //     //cout << "original:" << endl;
    //     for(auto i : arrFlow) {
    //         //cout << i << " ";
    //     }

    //     //cout << endl;
    // }

    for(auto index: arrIndex){
        Point p = prevPts[index];
        Point q = nextPts[index];
        fSumX += (p.x - q.x);
        fSumY += (p.y - q.y);
    }

    fRatio = (float)arrIndex.size()/arrFlow.size();

    if(arrIndex.size() == 0) {
        return Point(0,0);
    }

    if(fSumX/arrIndex.size() >= 50 || fSumY/arrIndex.size() >= 50) {
        return Point(0,0);
    }

    return Point(fSumX/arrIndex.size(), fSumY/arrIndex.size());
}

// static Point getXY_normal(const vector<Point2f>& prevPts, const vector<Point2f>& nextPts, const vector<uchar>& status)
// {
//     float fSumX = 0.f;
//     float fSumY = 0.f;
//     int iNum = 0;

//     for (size_t i = 0; i < prevPts.size(); ++i)
//     {
//         if (status[i])
//         {
//             Point p = prevPts[i];
//             Point q = nextPts[i];  
//             fSumX += (p.x - q.x);
//             fSumY += (q.y - q.y);

//             iNum++;
//         }
//     }

//     return Point(fSumX/iNum, fSumY/iNum); 
// }

static void drawArrows(Mat& frame, const vector<Point2f>& prevPtsOri, const vector<Point2f>& nextPtsOri, const vector<uchar>& status,  int iLenDisMin, Scalar line_color = Scalar(0, 0, 255))
{
    vector<float> arrFlow;
    vector<Point2f> prevPts;
    vector<Point2f> nextPts;

    for (size_t i = 0; i < prevPtsOri.size(); ++i){
        if (status[i]){

            Point p = prevPtsOri[i];
            Point q = nextPtsOri[i];

            prevPts.push_back(p);
            nextPts.push_back(q);

            arrFlow.push_back(sqrt((p.x - q.x)*(p.x - q.x)+(p.y - q.y)*(p.y - q.y)));
        }
    }


    auto arrIndex = GetXY<float>(arrFlow, iLenDisMin);

    for(auto& i: arrIndex){


        int line_thickness = 1;

        Point p = prevPts[i];
        Point q = nextPts[i];

        // //cout << (&i - &arrIndex[0]) << "@" << i << ":((" << p.x << "," << p.y << ")-(" << q.x << ","<< q.y<<"))=sqrt(" <<(p.x - q.x)*(p.x - q.x)+(p.y - q.y)*(p.y - q.y)<< ")     ";

        double angle = atan2((double) p.y - q.y, (double) p.x - q.x);

        double hypotenuse = sqrt( (double)(p.y - q.y)*(p.y - q.y) + (double)(p.x - q.x)*(p.x - q.x) );

        if (hypotenuse < 1.0)
            continue;

        // Here we lengthen the arrow by a factor of three.
        q.x = (int) (p.x - 3 * hypotenuse * cos(angle));
        q.y = (int) (p.y - 3 * hypotenuse * sin(angle));

        // Now we draw the main line of the arrow.
        line(frame, p, q, line_color, line_thickness);

        // Now draw the tips of the arrow. I do some scaling so that the
        // tips look proportional to the main line of the arrow.

        p.x = (int) (q.x + 9 * cos(angle + CV_PI / 4));
        p.y = (int) (q.y + 9 * sin(angle + CV_PI / 4));
        line(frame, p, q, line_color, line_thickness);

        p.x = (int) (q.x + 9 * cos(angle - CV_PI / 4));
        p.y = (int) (q.y + 9 * sin(angle - CV_PI / 4));
        line(frame, p, q, line_color, line_thickness);

    }

}


// static void drawArrows_bak(Mat& frame, const vector<Point2f>& prevPts, const vector<Point2f>& nextPts, const vector<uchar>& status, Scalar line_color = Scalar(0, 0, 255))
// {
//     double maxX = 0, minX = 0;
//     double maxY = 0, minY = 0;
//     for (size_t i = 0; i < prevPts.size(); ++i)
//     {
//         if (status[i])
//         {
//             int line_thickness = 1;

//             Point p = prevPts[i];
//             Point q = nextPts[i];

//             if(q.x > maxX) {
//                 maxX = q.x;
//             }
//             if(q.y > maxY) {
//                 maxY = q.y;
//             }            

//             if(q.x < minX) {
//                 minX = q.x;
//             }

//             if(q.y < minY) {
//                 minY = q.y;
//             }

//             double angle = atan2((double) p.y - q.y, (double) p.x - q.x);

//             double hypotenuse = sqrt( (double)(p.y - q.y)*(p.y - q.y) + (double)(p.x - q.x)*(p.x - q.x) );

//             if (hypotenuse < 1.0)
//                 continue;

//             // Here we lengthen the arrow by a factor of three.
//             q.x = (int) (p.x - 3 * hypotenuse * cos(angle));
//             q.y = (int) (p.y - 3 * hypotenuse * sin(angle));

//             // Now we draw the main line of the arrow.
//             line(frame, p, q, line_color, line_thickness);

//             // Now draw the tips of the arrow. I do some scaling so that the
//             // tips look proportional to the main line of the arrow.

//             p.x = (int) (q.x + 9 * cos(angle + CV_PI / 4));
//             p.y = (int) (q.y + 9 * sin(angle + CV_PI / 4));
//             line(frame, p, q, line_color, line_thickness);

//             p.x = (int) (q.x + 9 * cos(angle - CV_PI / 4));
//             p.y = (int) (q.y + 9 * sin(angle - CV_PI / 4));
//             line(frame, p, q, line_color, line_thickness);
//         }
//     }

//     //cout << "maxX:" << maxX << endl;
//     //cout << "maxY:" << maxY << endl;
//     //cout << "minX:" << minX << endl;
//     //cout << "minY:" << minY << endl;
// }

template <typename T> inline T clamp (T x, T a, T b)
{
    return ((x) > (a) ? ((x) < (b) ? (x) : (b)) : (a));
}

template <typename T> inline T mapValue(T x, T a, T b, T c, T d)
{
    x = clamp(x, a, b);
    return c + (d - c) * (x - a) / (b - a);
}

// static void getFlowField(const Mat& u, const Mat& v, Mat& flowField)
// {
//     float maxDisplacement = 1.0f;

//     for (int i = 0; i < u.rows; ++i)
//     {
//         const float* ptr_u = u.ptr<float>(i);
//         const float* ptr_v = v.ptr<float>(i);

//         for (int j = 0; j < u.cols; ++j)
//         {
//             float d = max(fabsf(ptr_u[j]), fabsf(ptr_v[j]));

//             if (d > maxDisplacement)
//                 maxDisplacement = d;
//         }
//     }

//     flowField.create(u.size(), CV_8UC4);

//     for (int i = 0; i < flowField.rows; ++i)
//     {
//         const float* ptr_u = u.ptr<float>(i);
//         const float* ptr_v = v.ptr<float>(i);


//         Vec4b* row = flowField.ptr<Vec4b>(i);

//         for (int j = 0; j < flowField.cols; ++j)
//         {
//             row[j][0] = 0;
//             row[j][1] = static_cast<unsigned char> (mapValue (-ptr_v[j], -maxDisplacement, maxDisplacement, 0.0f, 255.0f));
//             row[j][2] = static_cast<unsigned char> (mapValue ( ptr_u[j], -maxDisplacement, maxDisplacement, 0.0f, 255.0f));
//             row[j][3] = 255;
//         }
//     }
// }

int main(int argc, const char* argv[])
{


    const char* keys =
            "{ h            | help           | false | print help message }"
            "{ index        | index          |1      | specify cap index }"
            "{ width        | width          |640    | specify left image }"
            "{ height       | height         |480    | specify right image }"
            "{ fps          | fps            |30     | specify right image }"
            "{ gray         | gray           | false | use grayscale sources [PyrLK Sparse] }"
            "{ win_size     | win_size       | 21    | specify windows size [PyrLK] }"
            "{ max_level    | max_level      | 3     | specify max level [PyrLK] }"
            "{ iters        | iters          | 30    | specify iterations count [PyrLK] }"
            "{ points       | points         | 4000  | specify points count [GoodFeatureToTrack] }"
            "{ min_dist     | min_dist       | 0     | specify minimal distance between points [GoodFeatureToTrack] }"
            "{ lenDisMin    | lenDisMin      | 100   | specify lenDisMin }";

    CommandLineParser cmd(argc, argv, keys);

    if (cmd.get<bool>("help"))
    {
        //cout << "Usage: pyrlk_optical_flow [options]" << endl;
        //cout << "Avaible options:" << endl;
        cmd.printParams();
        return 0;
    }

    // string fname0 = cmd.get<string>("left");
    // string fname1 = cmd.get<string>("right");

    // if (fname0.empty() || fname1.empty())
    // {
    //     cerr << "Missing input file names" << endl;
    //     return -1;
    // }
    int index = cmd.get<int>("index");
    int width = cmd.get<int>("width");
    int height = cmd.get<int>("height");
    int fps = cmd.get<int>("fps");
    bool useGray = cmd.get<bool>("gray");
    int winSize = cmd.get<int>("win_size");
    int maxLevel = cmd.get<int>("max_level");
    int iters = cmd.get<int>("iters");
    int points = cmd.get<int>("points");
    double minDist = cmd.get<double>("min_dist");

    int iLenDisMin = cmd.get<int>("lenDisMin");

    bool bFlag = false;

    VideoCapture cap(index);
    // VideoCapture cap(VideoCapture capture("nvcamerasrc ! video/x-raw(memory:NVMM), width=(int)640, height=(int)480,format=(string)I420, framerate=(fraction)120/1 ! nvvidconv flip-method=2 ! video/x-raw, format=(string)BGRx ! videoconvert ! video/x-raw, format=(string)BGR ! appsink"););

    if(!cap.isOpened()) {
        //cout << "cannt open" << endl;

        /* code */
    }

    cap.set(CV_CAP_PROP_FRAME_WIDTH,width);
    cap.set(CV_CAP_PROP_FRAME_HEIGHT,height);
    cap.set(CV_CAP_PROP_FPS,fps);

    //cout << "CV_CAP_PROP_FRAME_WIDTH:" << cap.get(CV_CAP_PROP_FRAME_WIDTH);
    //cout << "CV_CAP_PROP_FRAME_HEIGHT:" << cap.get(CV_CAP_PROP_FRAME_HEIGHT);


    Mat frame0, frame1;
    Mat frameOri;
    cap >> frame0;
    frame0.copyTo(frame1);


    // Sparse  define
    PyrLKOpticalFlow d_pyrLK;

    d_pyrLK.winSize.width = winSize;
    d_pyrLK.winSize.height = winSize;
    d_pyrLK.maxLevel = maxLevel;
    d_pyrLK.iters = iters;

    co_chan<Mat> chMat(1);
    co_chan<Msg> chMsg;

    // shared_ptr<DealImage> imgDeal(new DealImage);

    flow_base_ns::Flow* flow = new flow_plugin_ns::Flow_OpenCV4Tegra;

    go [&]{
        while (true){
            cap >> frame0;
            chMat << frame0;
        }
    };

    go [=]{
        Mat mat ,matTmp = frame0;
        while(true){
            chMat >> mat;
            go [=]{
                print_now();
                cout << "before slotDealFrame " << endl;

                Msg msg /*= flow->compute_flow(mat, matTmp)*/;

                chMsg << msg;
            };

            go [=]{
                matTmp.copyTo(mat);
            } ;           

            go [=]{
                Msg msg;
                chMsg >> msg;

                go [=]{
                    cout << endl;
                    print_now();
                    cout << "msg.x:"<< msg.ix() << endl;
                    cout << "msg.y:"<< msg.iy() << endl;
                    cout << "msg.quality:"<< msg.iquality() << endl;

                };
            };
        }
    };

    co_sched.RunUntilNoTask();

    return 0;
}
